#pragma once
#include <Math/Vector4.hpp>
#include <Math/Vector3.hpp>
#include <Math/Vector2.hpp>
#include <Math/Matrix4x4.hpp>
#include <Math/Matrix3x3.hpp>
#include <Math/Matrix2x2.hpp>
#include <Graphics/Rotation.hpp>

namespace zzz{
// Get a D-dimensinal rotation matrix Mat, so that:
// Mat * from = to.
// There are unlimited such rotation matrix, since each vector only provide one constraints.
// There are D-3 DoF left. (since rotation matrix itself has 1 constraint)
// This process is just to find any one of them.
template<int D>
inline Matrix<D,D,double> GetRotationBetweenVectors(const Vector<D,double> &from, const Vector<D,double> &to)
{
  Matrix<D,D,double> rot;
  //actually this is not needed, algorithm can handle this
  //just to make it faster
  double dotlen=FastDot(from,to);
  if (Abs(dotlen-1)<EPSILON)
  {
    rot.Identical();
    return rot;
  }
  if (Abs(dotlen+1)<EPSILON)
  {
    rot.Identical();
    return -rot;
  }

  Matrix<D,D,double> e,alpha,beta;
  e.Identical();
  for (int i=0; i<D; i++) if (from[i]!=0) 
  {
    e.Row(i)=from; 
    if (i!=0) Swap(e.Row(0),e.Row(i));
    break;
  }
  GramSchmidt<double>::Process(e,alpha);
  e.Identical();
  for (int i=0; i<D; i++) if (to[i]!=0) 
  {
    e.Row(i)=to; 
    if (i!=0) Swap(e.Row(0),e.Row(i));
    break;
  }
  GramSchmidt<double>::Process(e,beta);

  for (int r=0; r<D; r++) for (int c=0; c<D; c++)
    rot(r,c)=FastDot(alpha.Row(r),beta.Row(c));
  rot=alpha.Inverted()*rot*alpha;
  return rot;
}

template<>
inline Matrix<3,3,double> GetRotationBetweenVectors(const Vector<3,double> &from, const Vector<3,double> &to)
{
  double costheta = FastDot(from, to);
  if (Within(1-TINY_EPSILON, Abs(costheta), 1+TINY_EPSILON)) {
    Vector3d axis = from.RandomPerpendicularUnitVec();
    Rotationd rot(Quaterniond(axis, SafeACos(costheta)));
    return rot;
  }
  Rotation<double> rot(from.Cross(to),SafeACos(FastDot(from,to)));
  return rot;
}

template<>
inline Matrix<2,2,double> GetRotationBetweenVectors(const Vector<2,double> &from, const Vector<2,double> &to)
{
  Vector3d from3(from,0),to3(to,0);
  Matrix<3,3,double> mat=GetRotationBetweenVectors(from3,to3);
  Matrix<2,2,double> ret(mat(0,0),mat(0,1),mat(1,0),mat(1,1));
  return ret;
}


}